/*
 * Created on 7 decembre 2006
 * bredeche(at)lri.fr
 *
 * This is demo for loading a map from an image file in Simbad . 
 * 
 * The important parts are:
 * - the main class, which describes the environment and setup the robot
 * - the robot class, which is embedded in the latter
 *  
 */

package simbad.contribs.maploader;

import java.io.FileNotFoundException;
import java.io.FileReader;
import java.util.ArrayList;

import javax.vecmath.Color3f;
import javax.vecmath.Point3d;
import javax.vecmath.Vector3d;

import simbad.gui.Simbad;
import simbad.sim.Agent;
import simbad.sim.BallAgent;
import simbad.sim.EnvironmentDescription;
import simbad.sim.LampActuator;
import simbad.sim.RangeSensorBelt;
import simbad.sim.RobotFactory;
import simbad.sim.Wall;


/**
 * This class represents an Environment useable in Simbad but with the 
 * specification of being defined by a PNG image. For the moment, the image
 * is defined as follows
 *  - 20x20 pixels (have to fit)
 *  - a red (rgb 255,0,0) pixel means a box
 *  - a green (rgb 0,255,0) pixel represent a goal to reach (if any)
 *  - a blue (rgb 0,0,255) pixel represents the starting position
 *  - dark gray (rgb 60,60,60) pixel representing a simulated ball
 *  - the image have to be in a PNG, JPG or GIF format.
 * 
 * TODO : 
 *  - define more flexible constraints (20x20 -> ~ [10:50]x[10;50]) or something like that
 *     (for now : ([0:20],[0:20]) values but it should be more likely ([10:20],[10:20]))
 *  - add a niveler due to the rgb values for more visual effects
 *  - add the specific (table) walls defined by some intermediary color. 
 * 
 * 
 * ADDED : 
 *  - multi floor : example : if you load image.png, the app will search for image-2.png
 *  for the second floor, image-3.png for the third, etc.
 *  - Multi-robot instances
 * 
 * @author nicolas (+ cedric)
 *
 */

public class MapLoaderBatteryDemo extends EnvironmentDescription {
	//static private Simbatch sim;
	//private Robot robot;
	
	// a goal to reach
	public Point3d _goal = new Point3d();
	public boolean _isGoal = false;
	// the initial starting points for robots (contains Point3d)
	public ArrayList _startingPoints = new ArrayList();
	public int _maxBattery = 5000;
	
	public MapLoaderBatteryDemo() 
	{
		// build the environment
		// here a simple square-closed environment.
		Wall w1 = new Wall(new Vector3d(9, 0, 0), 19, 1, this); w1.rotate90(1); add(w1);
		Wall w2 = new Wall(new Vector3d(-9, 0, 0), 19, 2, this); w2.rotate90(1); add(w2);
		Wall w3 = new Wall(new Vector3d(0, 0, 9), 19, 1, this); add(w3);
		Wall w4 = new Wall(new Vector3d(0, 0, -9), 19, 2, this); add(w4);
		
		// create the robot
		
		add(new Robot(new Vector3d(0, 0, 0), "MapRobot"));
		
	}
	
	/**
	 * initialize the environment due to the file __filename.
	 * TODO : 
	 *  - create multi-objectives points
	 *  - create balls or non static items. 
	 * @param __filename
	 */
	public MapLoaderBatteryDemo( String __filename ) 
	{
		int [] values;
		
		// initialise the image
		SimpleImage simpleImage = new SimpleImage (__filename, false);
		simpleImage.displayInformation();
		
		// step 2 : for each data in the image, initialize the environment
		//          + starting position (for one or more robots)
		//          + goal position
		for ( int y = 0 ; y != simpleImage.getHeight() ; y++ )
		{
			System.out.print(" ");
			for ( int x = 0 ; x != simpleImage.getWidth() ; x++ )
			{
				values = simpleImage.getPixel(x,y); 			
				if ( values[1] > 200 && values[2] < 50 && values[3] < 50 ) 
				{
					// red value, we will display a Wall in here : 
					add(new Wall(new Vector3d(x-(simpleImage.getWidth()/2),0,y-(simpleImage.getHeight()/2)),1,1,1,this));
					System.out.print("#");
				}
				else
					if ( values[1] < 50 && values[2] > 200 && values[3] < 50 )
					{
						// green value : define the goal point
						_goal.x = x-(simpleImage.getWidth()/2); _goal.z = y-(simpleImage.getHeight()/2); _goal.y = 0;
						_isGoal = true;
						System.out.print("X");
				        light1IsOn = true;
				        light1SetPosition(_goal.x,_goal.y,_goal.z);
				        light1Color = white;

					}
					else
						if ( values[1] < 50 && values[2] < 50 && values[3] > 200 )
						{
							_startingPoints.add(new Point3d(x,0,y));
							// starting position
							System.out.print("!");
						}
						else
							if (values[1]<100 && values[1]==values[2] && values[2]==values[3]){
								// add a ball
								// take care because the setUsePhysics remove the agentInspector
					            showAxis(false);
					            setUsePhysics(true);				            
		                        add(new BallAgent(new Vector3d(x-(simpleImage.getWidth()/2), 0, y-(simpleImage.getHeight()/2)), "ball", new Color3f(200,0,0),0.25f,0.25f));

							}
							else
							System.out.print(" ");
			}
			System.out.print("\n");
		}
		
		// step 3: second floor
		String secondFloor=__filename;
		boolean hasNextFloor = true;
		int cpt = 2;
		// add other floors to the environment
		// the other files should be called : for example if the initial file is maze.png
		//  - maze-2.png, maze-3.png, ....
		while(hasNextFloor){
			try {
				// step 3 : define a second floor if exists
				if (__filename.endsWith(".png")) secondFloor = __filename.replaceAll(".png","-"+cpt+".png");
				if (__filename.endsWith(".gif")) secondFloor = __filename.replaceAll(".gif","-"+cpt+".gif");
				if (__filename.endsWith(".jpg")) secondFloor = __filename.replaceAll(".jpg","-"+cpt+".jpg");
				// only way found to check if the file exist
				// if it does not exists an exception is raised and we can poursue without adding a second floor. 
				new FileReader(secondFloor); 
				
				// step 4 : initialise the image
				simpleImage = new SimpleImage (secondFloor, false);
				simpleImage.displayInformation();
				
				// step 5 : for each data in the image, update the environment
				for ( int y = 0 ; y != simpleImage.getHeight() ; y++ )
				{
					System.out.print(" ");
					for ( int x = 0 ; x != simpleImage.getWidth() ; x++ )
					{
						values = simpleImage.getPixel(x,y);
						if ( values[1] > 200 && values[2] < 50 && values[3] < 50 ) 
						{
							// red value, we will display a Wall in here : 
							add(new Wall(new Vector3d(x-(simpleImage.getWidth()/2),cpt-1,y-(simpleImage.getHeight()/2)),1,1,1,this));
							System.out.print("#");
						}
						else	System.out.print(" ");
					}
					System.out.print("\n");
				}
				cpt++;
			}
			catch(FileNotFoundException fnfe){ 
				// do nothing : do not add a second floor
				if (cpt==2){
					System.out.println("No second floor (optional).");
					System.out.println(" - to define a second floor, create a file called : "+secondFloor);
					System.out.println("");
				}
				hasNextFloor=false;
			}
		}
		
		//step 4 : add the robots of Robot instances
		for(int i=0;i<_startingPoints.size();i++)
			add(new Robot(new Vector3d(((Point3d)_startingPoints.get(i)).x-(simpleImage.getWidth()/2), 0f, ((Point3d)_startingPoints.get(i)).z-(simpleImage.getHeight()/2)), "robot n." + i,this));
	
	}

	
	// the robot is defined as an embedded class
	public class Robot extends Agent {
		
        LampActuator lamp;

		EnvironmentDescription _environment;
		RangeSensorBelt sonars, bumpers;
		int batteryLevel = -1;

		private boolean alive;
		
		
		public Robot(Vector3d position, String name) {
			super(position, name);
			
			// Add sensors
			sonars = RobotFactory.addSonarBeltSensor(this,8);
			bumpers = RobotFactory.addBumperBeltSensor(this,12);
            lamp = RobotFactory.addLamp(this);
            
            this.alive=true;
            batteryLevel = _maxBattery;
		}
		
		public Robot(Vector3d position, String name, EnvironmentDescription env) {
			super(position, name);
			
			this._environment = env;
			
			// Add sensors
			sonars = RobotFactory.addSonarBeltSensor(this,8);
			bumpers = RobotFactory.addBumperBeltSensor(this,12);
            lamp = RobotFactory.addLamp(this);

			this.alive=true;
			batteryLevel = _maxBattery;
		}
		
		/** Initialize Agent's Behavior */
		public void initBehavior() {
			batteryLevel = _maxBattery;
			this.alive=true;
		}
		
		/** Perform one step of Agent's Behavior */
		public void performBehavior() 
		{
			if ( this.alive == false ) return;
			
			Point3d MyCoord = new Point3d ();
			this.getCoords(MyCoord);
			//System.out.println(" x = " + MyCoord.x + " ; " + " y = " + MyCoord.y + " ; " + " z = " + MyCoord.z );
			//System.out.println("cible:  x = " + ((MapLoaderDemo2)_environment)._goal.x + " ; " + " y = " + ((MapLoaderDemo2)_environment)._goal.y + " ; " + " z = " + ((MapLoaderDemo2)_environment)._goal.z );
			//System.out.println("batterie : " + batteryLevel);
			if ( batteryLevel%50 == 0 && batteryLevel != 0 )
				System.out.println("# " + this.name + " - energy level is " + batteryLevel/10 + " %");
			
			double distanceToTarget = -1; 
			if ( ((MapLoaderBatteryDemo)this._environment)._isGoal == true )
				distanceToTarget =  
						Math.sqrt ( 
								( ((MapLoaderBatteryDemo)_environment)._goal.x - MyCoord.x ) * ( ((MapLoaderBatteryDemo)_environment)._goal.x - MyCoord.x ) ) + 
								( ((MapLoaderBatteryDemo)_environment)._goal.z - MyCoord.z ) * ( ((MapLoaderBatteryDemo)_environment)._goal.z - MyCoord.z ) ;
			
			// virtual battery - if close to recharge point, then recharge
			if ( /*((MapLoaderDemo2)this._environment)._isGoal == true && */distanceToTarget < 10.0 && batteryLevel <= _maxBattery - 1 )
			{
				batteryLevel+=2;
	            lamp.setBlink(true);
	            lamp.setOn(true);
			}
			else
			{
	            lamp.setBlink(false);
				lamp.setOn(true);
			}

			
			// controller (only if battery not discharged
			if ( batteryLevel > 0 )
			{
				batteryLevel--; // one step consumes one battery unit
					
				/*
				// *** clue-less robot ***
				if (collisionDetected()) {
					// stop the robot
					setTranslationalVelocity(0.0);
					setRotationalVelocity(0);
				}
				else
				{
					setTranslationalVelocity(1.5-2*Math.random());
					setRotationalVelocity(0.5-Math.random());
				}   
				*/
				
				// *** avoider robot ***
	            if (bumpers.oneHasHit()) {
	                setTranslationalVelocity(-0.1);
	                setRotationalVelocity(0.5-(0.1 * Math.random()));
	            } else if (collisionDetected()) {
	                // stop the robot
	                setTranslationalVelocity(0.0);
	                setRotationalVelocity(0);
	            } else   if (sonars.oneHasHit()) {
	                // reads the three front quadrants
	                double left = sonars.getFrontLeftQuadrantMeasurement();
	                double right = sonars.getFrontRightQuadrantMeasurement();
	                double front = sonars.getFrontQuadrantMeasurement();
	                // if obstacle near
	                if ((front  < 0.7)||(left  < 0.7)||(right  < 0.7)) {
	                    if (left < right)
	                        setRotationalVelocity(-1);
	                    else
	                        setRotationalVelocity(1);
	                    setTranslationalVelocity(0);
	                    
	                } else{
	                    setRotationalVelocity(0);
	                    setTranslationalVelocity(0.6);
	                }
	            } else {
	                setTranslationalVelocity(0.8);;
	                setRotationalVelocity(0);
	            }
	            	
			}
			else
			{
	            setRotationalVelocity(0);
	            setTranslationalVelocity(0.0);
	            lamp.setOn(false);
	            lamp.setBlink(false);
	            System.out.println("# " + this.name + " is dead.");
	            this.alive = false;
			}
		}
	}
	
	
	/*   	  
	 * for test purposes
	 */
	public static void main(String[] args) 
	{	
		MapLoaderBatteryDemo env = new MapLoaderBatteryDemo("simbad/resources/map1.png");
		//MapLoaderDemo2 env = new MapLoaderDemo2();
		new Simbad(env,false);	
	}
}